#include <ros/ros.h>
#include <iostream>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <geometry_msgs/TwistWithCovariance.h>
#include <std_msgs/Float64.h>
#include <nxt_msgs/Range.h>
#include <nxt_msgs/Gyro.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <stdio.h>

using namespace std;

/**************** COSTANTI E VARIABILI PER IL CALLBACK *****************/

double publication_frequency = 10; // frequenza a cui i sensori pubblicano le info, in Hz
double pi = 3.14159265358979323846;
double rot_speed = 1.55;
double linear_speed = 0.16;
double circle_linear_speed = 0.16; // v lineare di percorrenza del cerchio, v = w * r
double max_sonar_range = 1;
double distance_to_cone = 0.23;
double _angle_from_beginning = 0;


double gyro_angular_z = 0; // è costantemente aggiornata alla velocità angolare attuale del giroscopio
double gyro_angular_z_error = 0; // errore sistematico del giroscopio, viene aggiornato in fase di calibrazione
double gyro_integration = 0; // accumula nel tempo le misurazioni del giroscopio (quindi va diviso per la pubblication_frequency!)

double range = 2.5;

void coutVector(std::vector<double> v) {
	cout << "[ ";
	for(int i=0; i<v.size(); i++) {
		cout << v[i];
		if(i<v.size()-1)
		{
			cout << ",";
		}
		cout << " ";
	}
	cout << "]" << endl;
}

/***************** CALLBACK PER L'AGGIORNAMENTO DELLE VARIABILI *************/

/*
	Rende disponibili le informazioni sul giroscopio tramite
	la variabile globale gyro_angular_z.
	
	Si occupa anche di integrare nel tempo
	le informazioni sulle velocità angolari.

	Azzerando la variabile globale gyro_integration il conteggio
	dell'angolo percorso riparte da 0.
*/
void callbackGyro(const nxt_msgs::Gyro::ConstPtr& msg) {

	//cout << "Gyro angular velocity: " << msg->angular_velocity.z-gyro_angular_z_error << endl;
	gyro_angular_z = - msg->angular_velocity.z - gyro_angular_z_error;
	gyro_integration += gyro_angular_z/publication_frequency;

	_angle_from_beginning += gyro_angular_z/publication_frequency;

	//cout << "gro" << gyro_angular_z/publication_frequency << endl;
}

void callbackUltrasonicSensor(const nxt_msgs::Range::ConstPtr& msg)
{
	//cout << "Obstacle at " << msg->range << endl;
	range = msg->range;
}

/************************ END CALLBACKS *********************/




/************************ FUNZIONI DI CALIBRAZIONE **********/

/*
	Calibra il giroscopio facendo una media su un insieme di campioni acquisiti
	a robot fermo.

	samples: è il numero di campioni da utilizzare per la calibrazione.
*/
void calibrateGyro(int samples, ros::Rate r) {
	
	gyro_angular_z_error = 0; // inizio la calibrazione da un valore neutro

	double integrated_error = 0;
	double variance = 0;

	for(int i=0; i<samples; i++) { // non uso gyro_integration perchè non avrei controllo sul
								   //    numero di samples effettivamente acquisiti
 		
		integrated_error += gyro_angular_z;

		variance += gyro_angular_z*gyro_angular_z;
		//cout << integrated_error << " " << gyro_angular_z << endl;
		ros::spinOnce();
		r.sleep();
	}

	gyro_angular_z_error = integrated_error/samples;
	cout << "GIROSCOPE SYSTEMATIC ERROR: " << gyro_angular_z_error << endl;

	variance = variance/samples - integrated_error/samples*integrated_error/samples;
	cout << "GIROSCOPE SYSTEMATIC ERROR VARIANCE (high is bad): " << variance << endl;
}

/********************** END FUNZIONI DI CALIBRAZIONE *********/




/********************** FUNZIONI PER IL MOVIMENTO ************/

/*
	ferma il robot
*/
void stop(ros::Publisher velocity_pub, ros::Rate r) 
{

	geometry_msgs::Twist vel;
	velocity_pub.publish(vel);
   		
   	ros::spinOnce();
    r.sleep();

}

/*
	Ruota il robot di rad radianti basandosi sulle informazioni ottenute
	dal giroscopio per verificare l'angolo effettivamente percorso.
	
	Il controllo sulla velocità angolare del robot non garantisce step abbastanza
	fini di rotazione, quindi la funzione rotate() ritorna un double
	che indica l'angolo effettivamente percorso.

	Parametri
		double rad: indica di quanto il robot deve ruotare su se stesso (positivo --> senso antiorario)
*/
double rotate(ros::Publisher velocity_pub, ros::Rate r, double rad) {
	
	cout << "ROTATING BODY: " << rad*360/(2*pi) << "° ";

	double correction = 0.05; // radianti

	geometry_msgs::Twist vel;
	vel.angular.z = ( rad >= 0 ) ? rot_speed : -rot_speed; // la veloctià di rotazione viene mantenuta fissa e la direzione dipende dal segno dell'angolo
	rad = ( rad >= 0 ) ? rad-correction : rad+correction;

	gyro_integration = 0.0; // azzero l'integrale del giroscopio per misurare l'angolo percorso nella rotazione corrente
	
	while (abs(gyro_integration) < abs(rad))
   	{
   		//cout << "gyro_integration: " << gyro_integration << "target: " << rad << endl;
		velocity_pub.publish(vel);
   		ros::spinOnce();
   		r.sleep();
 	}

	stop(velocity_pub, r);

	//cout << "ROTATION ERROR: " << gyro_integration + rad << " rad";
	//cout << " --> " << 360*(gyro_integration + rad)/(2*pi) << " grad" << endl;

	cout << "EFFECTIVE ROTATION REACHED: " << -gyro_integration*360/(2*pi) << "°" << endl;

	return gyro_integration; // lo ritorno perchè è l'effettiva rotazione compiuta,
	                         // 	può differire di molto dalla rotazione richiesta!!
}

/*
	Questo metodo fa procedere il robot lungo una retta fino all'esaurimento dei secondi
	o al raggiungimento del target (range < = distance_to_cone)

	Parametri
		double seconds: se > 0 indica per quanti secondi devo avanzare, se invece è == 0 allora avanzo
						fino alla distance_to_cone fissata.

*/
double goStraight(ros::Publisher velocity_pub, ros::Rate r, double seconds = 0) {

	cout << "GOING STRAIGHT..." << endl;

	gyro_integration = 0; // azzero l'integrazione del giroscopio

	geometry_msgs::Twist vel;
	vel.linear.x = 0.05; // la velocità aumenta gradualmente a partire da un valore molto basso!

	bool withseconds = (seconds > 0);

	while ( (withseconds && seconds > 0 ) || ( !withseconds && range > distance_to_cone ) )
   	{
   		
   		if(withseconds)
   			seconds -= 1/publication_frequency;
   		else
   			seconds += 1/publication_frequency;

   		if (range <= distance_to_cone)
   		{
   			cout << "CONE REACHED !" << endl;
   			break;
   		}
   		
   		if(vel.linear.x < linear_speed) // la velocità lineare aumenta gradualmente fino al valore obiettivo massimo
   			vel.linear.x += 0.005;

   		vel.angular.z = gyro_integration/3.0; // setto la  velocità angolare perchè porti a zero l'integrale del giroscopio
		velocity_pub.publish(vel);
   		
   		ros::spinOnce();
   		r.sleep();
 	}

 	stop(velocity_pub, r);

 	return seconds;
}

/*
	Ruota la testa di un certo angolo

	Parametri
		double angle: l'angolo in gradi, positivo se senso orario, negativo se antiorario;
		double rotation_time: numero di secondi per raggiungere l'angolo richiesto;
*/
void rotateHead(ros::Publisher head_angle_pub, ros::Rate r, double angle, double rotation_time = 4) {

	cout << "ROTATING HEAD: " << angle << " °" << endl;

	std_msgs::Float64 head_angle;
	head_angle.data = angle;

	for (double t = 0; t <= rotation_time; t += 1/publication_frequency) // messaggi da pubblicare finchè la testa non ha girato dell'angolo richiesto
							  // perché non basta un solo publish
	{
		head_angle_pub.publish(head_angle);
		ros::spinOnce();
	    r.sleep();
	}

}

/*
	Parametri
		int direction: se > 0 la ricerca è fatta in senso antiorario, se < 0 la ricerca è fatta in senso orario;
		double obj_angle: è l'angolo a cui il robot si porta nel caso in cui non riesca ad individuare l'ostacolo;
		double prerotation: rotazione preliminare prima di iniziare la ricerca;

*/
bool lookForObstacle(ros::Publisher velocity_pub, ros::Publisher head_angle_pub, ros::Rate r, double obj_angle = 0) {
	
	cout << "LOOKING FOR OBSTACLE: " << endl;

	// check preliminare: ruota la testa e guarda in giro se vede qualcosa (range <= max_sonar_range)
	
	double head_start_angle = -90;
	double head_end_angle = 90;
	double head_step = 180/15.;
	bool found_something = false;
	int direction = 0;

	rotateHead(head_angle_pub, r, head_start_angle, 2);

	double a = 0;

	for(a = head_start_angle; a <= head_end_angle; a += head_step)
	{
		rotateHead(head_angle_pub, r, a, 0.3);
		if (range <= max_sonar_range)
		{
			found_something = true;
			direction = (a < 0) ? 1 : -1; // TODO: verificare orientazione angoli testa uguali a quelli corpo
			break;
		}
	}

	rotateHead(head_angle_pub, r, 0,4);

	// se non ha visto nulla ritorna false

	if(!found_something)
		return false;

	if(found_something && abs(a) <= 12)
		return true;

	// altrimenti cerca di orientare il corpo nella direzione del birillo

	double step = pi/15;
	
	double total_angle = 0;
	double effective_rotation = 0;
	
	double minimum = range;
	vector<double> equal_sequence_steps;
	
	bool found = false;
	double tolerance = pi/2; // tolleranza di superamento dell'angolo obj_angle durante la ricerca
	
	//effective_rotation = rotate(velocity_pub, r, prerotation);
	//total_angle += effective_rotation;
	
	while(!found) {

		if ( abs(total_angle) > abs(obj_angle + tolerance) )
			break;

		effective_rotation = rotate(velocity_pub, r, step*direction);
		total_angle += effective_rotation;
		
		//cout << minimum << " " << range << endl;
		
		if (range == minimum)
		{
			equal_sequence_steps.push_back(effective_rotation);
		}
		else if (range < minimum || range > max_sonar_range)
		{
				minimum = range;
				//cout << "clearing sequence" << endl;
				equal_sequence_steps.clear();
		}
		else
		{		
			//cout << "Minimum reached!" << endl;
			stop(velocity_pub, r);
			equal_sequence_steps.push_back(effective_rotation);
			found = true;
		}

		//coutVector(equal_sequence_steps);
	}
	
	if (found)
	{
		// A questo punto ho individuato il cono, equal_sequence_steps contiene tutti gli step che
		// ho fatto da quando sono arrivato al cono più l'ultimo step che mi ha fatto superare il
		// cono. Quindi se voglio centrare il robot sul cono devo ripercorrere all'indietro la lista degli
		// spostamenti con range minimo fatti fin'ora.

		// controllo se equal_sequence_steps.size() è pari o dispari
		bool even = (equal_sequence_steps.size()%2 == 0) ? true : false;

		//cout << "even: " << even << endl;

		double angle_to_cone = 0;
		angle_to_cone = even ? equal_sequence_steps[equal_sequence_steps.size()/2-1] : 0 ;

		for (int j = equal_sequence_steps.size()-1; j >= (int)equal_sequence_steps.size()/2; j--) {
			angle_to_cone += equal_sequence_steps[j];
		}

		//cout << "angle_to_cone: " << angle_to_cone << endl;

		rotate(velocity_pub, r, angle_to_cone);
		// TODO: se fa un errore troppo grande potrei prevedere degli aggiustamenti in retroazione sull'angolo!
	}
	else
	{
		rotate(velocity_pub, r, total_angle + obj_angle);
		// TODO: verificare correttezza, ma direi che va bene perchè se ruoto total_angle torno a zero,
		//		poi sommo l'obj_angle.
	}

	return found;
}


/*
	Descrive un arco di circonferenza di fissato raggio r, 
	con velocità lineare v e velocità angolare w tali che v = w * r
	
	il robot gira attorno a un ostacolo a una distanza "fissa" 
	la velocità angolare è regolata in base al range restituito dal sonar
	se range < radius, la velocità angolare viene diminuita
	se range > radius, la velocità angolare viene aumentata

	Parametri
		double radius: il raggio dell'arco descritto;
		double rad: l'ampiezza dell'arco in radianti;
*/
double moveInCircle(ros::Publisher velocity_pub, ros::Rate r, double radius, double rad) {

	cout << "MOVING IN CIRCLE: " << radius << " radius, "<< rad << " rad" << endl;

	// velocità base
	geometry_msgs::Twist vel;
	vel.linear.x = circle_linear_speed; // v, for example 0.1 0.2 0.3 0.4 0.5 1
	double w = vel.linear.x / radius; // velocità angolare radius = 0.3 -> 0.33 0.67 1.33 1.67 3.33
	vel.angular.z = ( rad >= 0 ) ? w: -w; // w = v / radius
	
	// velocità per correzione retroattiva
	geometry_msgs::Twist corrected_vel;
	corrected_vel.linear.x = vel.linear.x;
	corrected_vel.angular.z = vel.angular.z;

	double correction = 1.1; // > 1

	gyro_integration = 0; // azzero l'integrazione del giroscopio
	double described_angle = 0;
	while (abs(gyro_integration) < abs(rad) /*&& abs(described_angle) < abs(rad)*/) // usa il giroscopio, l'angolo teorico è impreciso
	{
		if (range == radius) // situazione ideale
		{
			velocity_pub.publish(vel);
		}
		else if ( range < radius ) // se r diminuisce, w da diminuire
		{
			corrected_vel.angular.z = vel.angular.z * (correction - 1);
			velocity_pub.publish(corrected_vel);
			//cout << "range < 0.3 " << range << " corrected_vel_lin_x " << corrected_vel.linear.x 
			//	 					  		<< " corrected_vel_ang_z " << corrected_vel.angular.z  << endl;
		}
		else if ( range > radius ) // se r aumenta, w da aumentare
		{
			corrected_vel.angular.z = vel.angular.z * correction;
			velocity_pub.publish(corrected_vel);
			//cout << "range > 0.3 " << range << " corrected_vel_lin_x " << corrected_vel.linear.x 
			//	 					 		<< " corrected_vel_ang_z " << corrected_vel.angular.z  << endl;
		}
		else // la testa non inquadra il cono e range = 2.55, riesce comunque a evitarlo
		{
			velocity_pub.publish(vel);
		}

		described_angle += vel.angular.z / publication_frequency; // angolo teorico
		ros::spinOnce();
   		r.sleep();
   	}

   	//cout << "THEORETICAL DESCRIBED ANGLE: " << described_angle << endl;
   	cout << "DESCRIBED ANGLE BY GIRO: " << gyro_integration << endl;

   	return gyro_integration;

}

/********************** END FUNZIONI PER IL MOVIMENTO **************************/





/******************************* MAIN FUNCTION *********************************/

int main(int argc, char **argv) {
	
	ros::init(argc, argv, "lab1");
	
	ros::Subscriber gyro_sub, ultrasonic_sensor_sub;
	ros::Publisher velocity_pub, head_angle_pub;
	
	ros::NodeHandle node_handler_subscribers, node_handler_publishers;

	ros::Rate r(publication_frequency); // frequenza di spin in hertz
	
	gyro_sub = node_handler_subscribers.subscribe("gyro", 100, callbackGyro); // name, queue size, callback
	ultrasonic_sensor_sub = node_handler_subscribers.subscribe("ultrasonic_sensor", 100, callbackUltrasonicSensor);
	
	velocity_pub = node_handler_publishers.advertise<geometry_msgs::Twist>("cmd_vel", 1);
	head_angle_pub = node_handler_publishers.advertise<std_msgs::Float64>("angle", 100);

	
	double travelling_time = 0.7/linear_speed;

	cout << "CALIBRATING GYROSCOPE..." << endl;
	
	// calibro il giroscopio
	calibrateGyro(200, r);

	_angle_from_beginning = 0;


	// avanzo fino al primo cono e memorizzo per quanti secondi ho avanzato
	double first_segment_duration = goStraight(velocity_pub, r);

	cout << "	ANGLE FROM BEGINNING: " << _angle_from_beginning*360./(2*pi) << " EXPECTED: 0° "  << endl;	

	rotate(velocity_pub, r, pi/2); // +rad -> +w -> senso anti orario (meglio se |rad| -> -w -> senso orario)
    rotateHead(head_angle_pub, r, 90); // 100 -> 90° (non sempre) senso orario
    moveInCircle(velocity_pub, r, distance_to_cone+0.03, -pi); // -pi/2 -> senso orario
    stop(velocity_pub, r);
    rotateHead(head_angle_pub, r, 0);
    rotate(velocity_pub, r, pi/2*0.9);

    cout << "	ANGLE FROM BEGINNING: " << _angle_from_beginning*360./(2*pi) << " EXPECTED: 0°" << endl;

    stop(velocity_pub, r);

    // cerco di vedere il prossimo cono, controllando ad intervalli regolari
    while(!lookForObstacle(velocity_pub, head_angle_pub, r, 1))
		goStraight(velocity_pub, r, travelling_time*2/3.);
	// se ho visto il cono avanzo fino a raggiungerlo
	goStraight(velocity_pub, r);

	sleep(8);

	//calibrateGyro(100, r);

	// ruoto di 180°
	rotate(velocity_pub, r, -pi);

	cout << "	ANGLE FROM BEGINNING: " << _angle_from_beginning*360/(2*pi) << " EXPECTED: 180°" << endl;
	
	// ritorno cercando il cono di partenza
	while(!lookForObstacle(velocity_pub, head_angle_pub, r, 1))
		goStraight(velocity_pub, r, travelling_time*2/3);
	goStraight(velocity_pub, r);

	cout << "	ANGLE FROM BEGINNING: " << _angle_from_beginning*360/(2*pi) << " EXPECTED: 180°" << endl;

	// ruoto nuovamente attorno al primo cono.
	rotate(velocity_pub, r, pi/2); // +rad -> +w -> senso anti orario (meglio se |rad| -> -w -> senso orario)
    rotateHead(head_angle_pub, r, 90); // 100 -> 90° (non sempre) senso orario
    moveInCircle(velocity_pub, r, distance_to_cone+0.03, -pi); // -pi/2 -> senso orario
    stop(velocity_pub, r);
    rotateHead(head_angle_pub, r, 0);
    rotate(velocity_pub, r, pi/2);

    cout << "	ANGLE FROM BEGINNING: " << _angle_from_beginning*360/(2*pi) << " EXPECTED: 180°" << endl;

   	// ora utilizzo il tempo di avanzamento registrato all'inizio per tornare alla posizione di partenza.
    goStraight(velocity_pub, r, first_segment_duration);
}

/**************************** END MAIN FUNCTION *********************************/